#ifndef COMPASS_H
#define COMPASS_H

#ifdef __cplusplus
extern "C" {
#endif

#include <stdint.h>
#include "formatted_io.h"
	
void compass_init();
void compass_read(int16_t *mag);


	//////////////////
	
	typedef struct vector
	{
		float x, y, z;
	} vector;
	
	extern void vector_cross(const vector *a, const vector *b, vector *out);
	extern float vector_dot(const vector *a,const vector *b);
	extern void vector_normalize(vector *a);
	
	class compass
	{
	public:
		vector a; //accelerometer	
		vector m; //magnetometer
		
		// offset correction in degrees
		float correctionInDegrees;
		
		// normalize ranges
		vector m_max;
		vector m_min;
		
		float heading(void);
		float heading(vector from);
		void find_extremes(formatted_io& BT);
		float heading_pitch_roll(float pitch, float roll, formatted_io &BT);
		
		// constructor
		compass() {
			// These are just some values for a particular unit, it is recommended that
			// a calibration be done for your particular unit.
			m_max.x = +642; m_max.y = +689; m_max.z = 797;
			m_min.x = -666; m_min.y = -607; m_min.z = -456;
			
			correctionInDegrees = 0.0;
		}
		
	};
#ifdef __cplusplus
}
#endif
#endif
